/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "mccontrollerthread.h"

#include <kdl/frames_io.hpp>
#include <QCoreApplication>


//#define IF_DEBUG true
#define IF_DEBUG false


McControllerThread::McControllerThread(int robotIdIn, RegData *regDataIn, RobotIo *ioControllerIn) :
    robotId(robotIdIn)
{

    mcControllerInterpreter = new McControllerInterpreter(robotId,regDataIn,ioControllerIn);

#ifdef D_USE_MOTION_UNIT_TEST



#endif



}

McControllerInterpreter * McControllerThread::getControllerInterpreter()
{
    return mcControllerInterpreter;
}

int McControllerThread::getRobotId()
{
    return robotId;
}

int McControllerThread::setMotorControlMode(int axisId, E_MOTOR_CONTROL_MODE modeIn)
{

    return mcControllerInterpreter->setMotorControlMode(axisId,modeIn);
}

int McControllerThread::algorithmFunctioncall(const QVector<bool> &servoOnStatus, const QVector<MotorData> &motorFeedback,
                                              QVector<MotorData> &motorCommandOut, int &returnMutexFlag)
{
    return mcControllerInterpreter->algorithmFunctioncall(servoOnStatus,motorFeedback,motorCommandOut,returnMutexFlag);
}

int McControllerThread::initialServoParameter()
{
    return mcControllerInterpreter->initialServoParameter();
}

//void McControllerThread::run()
//{
//    struct sched_param param;

//    param.__sched_priority = 96;
//    sched_setscheduler(0,SCHED_FIFO,&param);

//    int policy,priority;
//    pthread_getschedparam(pthread_self(),&policy,(struct sched_param *)&priority);



//    while(false==mcControllerInterpreter->getInitialStatus())
//    {
//        sleep(1);
//    }


//    while (1) //需改为条件唤醒
//    {
//        mcControllerInterpreter->updateStatus();
//        mcControllerInterpreter->motionServerLoopCommon();
//        //this->usleep(5000);//休眠5ms
//        this->usleep(100000);//休眠100ms

//    }

//    exec();
//}



